# 转化增广矩阵Aa，Ba，Sa，Qa，权重矩阵R，稳态控制输入ud
import numpy as np
def F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd):
    n = len(A)
    p = B.shape[1]
    Ca = np.hstack((np.eye(n),-np.eye(n)))#横向堆叠
    
    Aa = np.vstack((np.hstack((A,np.eye(n)-A)),np.hstack((np.zeros((n,n)),np.eye(n)))))

    Ba = np.vstack((B,np.zeros((n,p))))
    Qa = Ca.T@Q@Ca
    Sa = Ca.T@S@Ca
    R = R
    ud = np.linalg.lstsq(B,(np.eye(n)-A)@xd.T,rcond=None)[0]
    return Aa,Ba,Qa,Sa,R,ud

    # np.vstack()  纵向堆叠
    # np.hstack()  横向堆叠
    # np.eye(n)    单位矩阵
    #(I-A)xd = B*ud
    # 确保B是方阵且可逆，否则np.linalg.solve报错。如果B不是方阵，可以使用 np.linalg.lstsq 进行最小二乘求解。












